#include <arch/cpu/soc/irqnr.inc>
#include <arch/arm.h>

.globl __vectors_start
__vectors_start:

	ldr pc, reset_vector
	ldr pc, undef_vector
	ldr pc, swi_vector
	ldr pc, iabort_vector
	ldr pc, dabort_vector
	ldr pc, hang_vector
	ldr pc, irq_vector
	ldr pc, fiq_vector

reset_vector:	.word reset_handler
undef_vector:	.word undef_handler
swi_vector:		.word swi_handler
iabort_vector:	.word iabort_handler
dabort_vector:	.word dabort_handler
hang_vector:	.word hang_handler
irq_vector:		.word irq_handler
fiq_vector:		.word fiq_handler

.globl __vectors_end
__vectors_end:

reset_handler:
undef_handler:
swi_handler:
iabort_handler:
dabort_handler:
hang_handler:
fiq_handler:
    /* disable IRQ FIQ */
	mrs r0, cpsr
	orr r0, r0, #ARM_INT_MASK
	msr cpsr_c, r0
	b .

irq_handler:
	sub lr, lr, #4
	stmfd sp, {r0, lr}			@ save r0, lr_irq (parent pc)
	mrs lr, spsr
	str lr, [sp, #-12]			@ save spsr_irq (parent cpsr)
	mov r0, sp					@ r0 = sp_irq

	@ switch to SVC mode
	mrs lr, cpsr
	eor lr, lr, #(ARM_MODE_IRQ ^ ARM_MODE_SVC)
	msr cpsr_c, lr	

	str lr, [sp, #-8]!			@ save lr_svc
	ldr lr, [r0, #-4]			
	str lr, [sp, #4]			@ save parent pc (lr_irq)
	ldr lr, [r0, #-12]			@ get parent cpsr (spsr_irq)
	ldr r0, [r0, #-8]			@ get r0
	stmfd sp!, {r0 - r12}
	stmfd sp!, {lr}				@ save parent cpsr
	
	ldr r0, =OSIntNestingCtr
	ldr r1, [r0]
	add r1, r1, #1
	str r1, [r0]
	
	cmp r1, #1
	ldreq r0, =OSTCBCurPtr
	ldreq r0, [r0]
	streq sp, [r0]
	
	get_irqnr_preamble r2, lr
1:	get_irqnr_and_base r0, r1, r2, lr
	adrne lr, 1b
    bne do_irq 

	bl OSIntExit

    ldmfd sp!, {r0}                  @ pop new task's cpsr     
    msr spsr_cxsf, r0
    ldmfd sp!, {r0 - r12, lr, pc}^   @ pop new task's r0-r12,lr & pc 


